#ifndef HERO_VISION__HERO_VISION_NODE_HPP_
#define HERO_VISION__HERO_VISION_NODE_HPP_
// ROS 2核心组件
#include <image_transport/image_transport.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/string.hpp>

// 标准库
#include <memory>
#include <string>
#include <vector>
#include <unordered_map>

// 自定义模块
#include "hero_integrated_vision/armor.hpp"                  // 装甲板/灯条数据结构
#include "hero_integrated_vision/base_detector.hpp"          // 检测器基类
#include "hero_integrated_vision/cameraparams_manager.hpp"
#include "aim_interfaces/msg/armors.hpp"  // 装甲板消息类型
#include "aim_interfaces/msg/armor.hpp"

namespace rm_aim
{

/**
 * @brief 英雄视觉节点，负责装甲板检测与位姿发布
 * @details 接收相机图像和内参，通过传统视觉算法检测装甲板，
 *          计算3D位姿并发布检测结果
 */
class HeroVisionNode : public rclcpp::Node
{
public:
  /**
   * @brief 构造函数，初始化节点组件
   * @param options ROS 2节点选项
   */
  HeroVisionNode(const rclcpp::NodeOptions & options);

private:
  // -------------------------- 策略模式核心声明 --------------------------
  // 策略接口：统一所有模式处理入口
  class AimStrategy {
  public:
    virtual ~AimStrategy() = default;
    virtual void doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) = 0;
  protected:
    AimStrategy(HeroVisionNode& node) : node_(node) {}
    HeroVisionNode& node_; // 持有节点引用，访问检测器、发布者等资源
  };

  // 具体策略类：每个模式对应一个策略
  class CloseAimStrategy : public AimStrategy {
  public:
    CloseAimStrategy(HeroVisionNode& node) : AimStrategy(node) {}
    void doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) override;
  };

  class NormalAimStrategy : public AimStrategy {
  public:
    NormalAimStrategy(HeroVisionNode& node) : AimStrategy(node) {}
    void doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) override;
  };

  class OutpostStrategy : public AimStrategy {
  public:
    OutpostStrategy(HeroVisionNode& node) : AimStrategy(node) {}
    void doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) override;
  };

  class GreenLightStrategy : public AimStrategy {
  public:
    GreenLightStrategy(HeroVisionNode& node) : AimStrategy(node) {}
    void doProcess(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) override;
  };

  /**
   * @brief 切换策略
   * @param mode 模式名称
   */
  void switchStrategy(const std::string& mode); // 切换策略
 
  // 策略管理：映射（模式名→策略实例）+ 当前激活策略
  std::unordered_map<std::string, std::unique_ptr<AimStrategy>> strategy_map_;
  AimStrategy* current_strategy_;

  // 控制指令订阅者（用于动态切换策略）
  rclcpp::Subscription<aim_interfaces::msg::GimbalControl>::SharedPtr control_sub_;
  // -------------------------- 图像处理-------------------------
  /**
   * @brief 图像回调函数，处理输入图像并执行检测流程
   * @param img_msg 输入图像消息
   */
  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg);

  /**
   * @brief 初始化检测器（传统视觉检测器）
   * @return 初始化完成的检测器智能指针
   */
  std::unique_ptr<BaseDetector> initDetector();

  /**
   * @brief 检测装甲板核心逻辑
   * @param img_msg 输入图像消息
   * @return 检测到的装甲板列表
   */
  std::vector<Armor> detectArmors(const sensor_msgs::msg::Image::ConstSharedPtr & img_msg);

  /**
   * @brief 检测绿灯
   * @param img 输入图像
   * @return 检测到的绿灯列表
   */
  std::vector<cv::Rect2f> detectGreenLights(const cv::Mat& img);

  /**
   * @brief 绘制检测结果
   * @param img 输入图像
   * @param green_lights 检测到的绿灯列表
   */
  void drawGreenLightResults(cv::Mat& img, const std::vector<cv::Rect2f>& green_lights); 

  // 装甲板检测器（传统视觉实现）
  std::unique_ptr<BaseDetector> traditional_detector;

  // 检测结果发布者
  aim_interfaces::msg::Armors armors_msg_;  // 装甲板消息容器
  rclcpp::Publisher<aim_interfaces::msg::Armors>::SharedPtr armors_pub_;  // 消息发布者

  // 图像订阅与结果发布
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_sub_;  // 图像订阅者
  image_transport::Publisher result_img_pub_;  // 结果图像发布者

  // 内参管理
  std::unique_ptr<CameraParamsManager> cameraparams_manager_;
};

}  // namespace rm_aim

#endif  // HERO_VISION__HERO_VISION_NODE_HPP_